(Field of the Invention)
The present invention relates to a control device and a control method for a link actuation device to be used in an apparatus, such as an industrial apparatus, that requires a precise and wide operating range.
(Description of Related Art)
As link actuation devices that fast and precisely perform work such as complicated working or processing in a three-dimensional space, there are devices that each include three or more quadric chain link mechanisms to allow movement of two degrees of freedom (for example, Patent Documents 1 to 3 listed below). As shown in FIG. 9, for example, each of such link actuation devices includes: a proximal end side link hub 14; a distal end side link hub 15; and three link mechanisms 11, 12, and 13 which connect these link hubs 14 and 15 to each other. Each link mechanism 11, 12, 13 is composed of: a proximal side end link member 11a, 12a, 13a; a distal side end link member 11b, 12b, 13b; and an intermediate link member 11c, 12c, 13c. Although the proximal side end link member 13a is not shown in FIG. 9, a reference sign is used in the explanation for identification thereof. Hereinafter, the proximal side end link members 11a, 12a, and 13a will be referred to as arms 11a, 12a, and 13a, respectively.
A link actuation device of this type drives three or more link mechanisms 11, 12, and 13 by means of two or more actuators (not shown) such as motors, thereby changing the posture of the distal end side link hub 15 relative to the proximal end side link hub 14 (hereinafter, referred to as “link hub distal end posture”, or simply referred to as “posture”). The link hub distal end posture is determined by a bend angle θ and an angle of traverse ϕ. The bend angle θ is the inclination angle of a central axis QB of the distal end side link hub 15 relative to a central axis QA of the proximal end side link hub 14, and the angle of traverse ϕ is the angle of traverse of the central axis QB of the distal end side link hub 15 relative to the central axis QA of the proximal end side link hub 14.
Specifically, in order to control the link hub distal end posture, the rotation angle β1n, β2n, β3n of the arm 11a, 12a, 13a are obtained from the bend angle θ and the angle of traverse ϕ, and the actuator that drives the arm 11a, 12a, 13a is caused to determine the position thereof. In the description below, the rotation angle of each arm will be referred to as “arm rotation angle”. For example, with respect to a posture A(θa, ϕa) and a posture B(θb, ϕb) which is different from the posture A shown in FIG. 10, the arm rotation angles respectively corresponding to the postures A and B are obtained as A(β1a, β2a, β3a) and B(β1b, β2b, β3b) by the following Formula (1) which represents the relationship between the bend angle θ and the angle of traverse ϕ, and the arm rotation angle β.cos(θ/2)sin βn−sin(θ/2)sin(ϕ+δn)cos βn+sin(γ/2)=0:  Formula (1)
γ is the angle between the axis of the connection end of the intermediate link member 11c, 12c, 13c rotatably connected to the arm 11a, 12a, 13a and the axis of the connection end of the intermediate link member 11c, 12c, 13c rotatably connected to the distal side end link member 11b, 12b, 13b. δn (δ1, δ2, δ3) (not shown) is the separation angle in the circumferential direction of the proximal side end link member 11a, 12a, 13a relative to an arm 11a serving as a reference. When the number of the link mechanisms 11, 12, and 13 is three and the link mechanisms 11, 12, and 13 are arranged at an equal interval in the circumferential direction, the separation angles δ1, δ2, and δ3 of the arms 11a, 12a, and 13a are 0°, 120°, 240°, respectively.
Here, change in posture from the posture A to the posture B is realized by the rotation angles of the arms 11a, 12a, and 13a changing from β1a to β1b, from β2a to β2b, and from β3a to β3b, respectively.
In Patent Document 4 listed below, from a rectangular coordinate system which is perpendicular to the central axis QA of the proximal end side link hub 14 and which is set on the extended line of the central axis QA of the proximal end side link hub 14, the posture (bend angle θ, angle of traverse ϕ) of the link hub is obtained by using convergence calculation by the method of least squares. This enables positioning of an end effector such as a working machine to an arbitrary coordinates on a work surface (rectangular coordinate plane) on which the end effector works.